#include <mutex>
#include <thread>
#include <string>
#include <iostream>
#include <pcl/point_cloud.h>
#include <pcl/visualization/pcl_visualizer.h>

class PointVis
{
    private:
        bool isStop;                                            // 判断是否终止
        bool isUpdate;                                          // 判断点云数据是否更新
        std::mutex mutexUpdate;                                 // 互斥锁
        pcl::PointCloud<pcl::PointXYZ>::Ptr pointcloud_ptr;     // 保存点云数据

    public:
        PointVis();
        ~PointVis();

        void Vis();                                             // 显示点云
        void SetStop();                                         // 设置停止位
        void UpdatePoints(float x, float y, float z);           // 更新点云
};

PointVis::PointVis()
{
    isStop = false;
    isUpdate = false;
    std::cout<<"isStop is set false"<<std::endl;
    pointcloud_ptr.reset(new pcl::PointCloud<pcl::PointXYZ>());
    pcl::PointXYZ p;
    p.x = 0;
    p.y = 0;
    p.z = 0;
    pointcloud_ptr->push_back(p);
    pointcloud_ptr->width = 1;
    pointcloud_ptr->height = 1;
    std::cout<<"pointcloud_ptr init"<<std::endl;
}

PointVis::~PointVis()
{
}

/* 设置显示模块停止 */
void PointVis::SetStop()
{
    std::lock_guard<std::mutex> lck(mutexUpdate);
    isStop = true;
    std::cout<<"isStop is set true"<<std::endl;
    return;
}

/* 点云数据更新 */
void PointVis::UpdatePoints(float x, float y, float z)
{
    std::lock_guard<std::mutex> lck(mutexUpdate);

    for(float i = -x; i < x; i+=0.1)
    {
        for (float j = -y; j < y; j+=0.1)
        {
            pcl::PointXYZ p;
            p.x = i;
			p.y = j;
			p.z = z;
            pointcloud_ptr->push_back(p);
        }
    }
    pointcloud_ptr->width = pointcloud_ptr->size();
    pointcloud_ptr->height = 1;
    isUpdate = true;
    std::cout<<"UpdatePoints finished"<<std::endl;
}

/* 点云显示 */
void PointVis::Vis()
{
    std::cout<<"thread begin!"<<std::endl;

    // viewer实例化
    pcl::visualization::PCLVisualizer viewer("viewer");
    viewer.setBackgroundColor(0,0,0);
    viewer.addCoordinateSystem(1);
    viewer.addPointCloud(pointcloud_ptr, "sample cloud");
    //viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "sample cloud");
    //viewer.initCameraParameters();
    std::cout<<"viewer init!"<<std::endl;

    while (1)
    {
        if (isStop)
        {
            std::cout<<"viewer break!"<<std::endl;
            break;
        }
        else
        {
            std::lock_guard<std::mutex> lck(mutexUpdate);
            if (isUpdate)
            {
                // 点云刷新
                viewer.updatePointCloud<pcl::PointXYZ>(pointcloud_ptr, "sample cloud");
                isUpdate = false;
            }
            
            viewer.spinOnce(10);   // 等待10毫秒
        }
    }
    
}

int main()
{
    int s;
    float x, y, z;
    
    PointVis pv;

    // 创建子线程,并传入参数 传地址, 
    // 子线程为类里面的一个成员函数,都传地址
    std::thread th(&PointVis::Vis, &pv);

    while (1)
    {
        std::cout<<"输入66退出,输入其他更新点云数据"<<std::endl;
        cin>>s;

        if (s == 66)
        {
            pv.SetStop();  //  isStop = true; pv子线程上锁 
            break;  // 退出循环
        }
        else
        {
            std::cout<<"开始更新点云数据"<<std::endl;
            float x, y, z;
            std::cout<<"输入x:"<<std::endl;
            cin>>x;
            std::cout<<"输入y:"<<std::endl;
            cin>>y;
            std::cout<<"输入z:"<<std::endl;
            cin>>z;
            pv.UpdatePoints(x, y, z);  // 更新点云成功
        }

    }
    th.join();

    return 0;
}
